package com.hitqz.robot.driver.util;

import cn.hutool.core.date.DateTime;
import cn.hutool.core.date.DateUnit;
import cn.hutool.core.date.DateUtil;
import cn.hutool.core.io.FileUtil;
import cn.hutool.core.net.NetUtil;
import cn.hutool.core.util.ByteUtil;
import cn.hutool.core.util.StrUtil;
import cn.hutool.extra.spring.SpringUtil;
import com.google.common.collect.ArrayListMultimap;
import com.google.common.collect.Multimap;
import com.hitqz.robot.api.common.dto.robot.RobotOnlineEventDto;
import com.hitqz.robot.api.common.entity.Robot;
import com.hitqz.robot.api.common.entity.RobotMap;
import com.hitqz.robot.api.common.entity.RobotMapPos;
import com.hitqz.robot.driver.communication.NetworkServiceThreadPool;
import com.hitqz.robot.driver.communication.UdpClient;
import com.hitqz.robot.driver.dto.KcNavStatus;
import com.hitqz.robot.driver.dto.KcRobotRunningStatus;
import com.hitqz.robot.driver.netty.handler.KcMessageHandler;
import com.hitqz.robot.driver.netty.protocol.decoder.KcDecoder;
import com.hitqz.robot.driver.netty.protocol.message.KcMessage;
import io.netty.channel.ChannelInitializer;
import io.netty.channel.socket.nio.NioDatagramChannel;
import lombok.extern.slf4j.Slf4j;
import org.dom4j.Document;
import org.dom4j.DocumentException;
import org.dom4j.DocumentHelper;
import org.dom4j.Element;

import java.io.BufferedReader;
import java.io.File;
import java.io.InputStreamReader;
import java.math.BigDecimal;
import java.math.RoundingMode;
import java.net.InetAddress;
import java.net.NetworkInterface;
import java.net.SocketException;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.nio.charset.StandardCharsets;
import java.util.*;
import java.util.concurrent.*;

/**
 * @author wengyx
 * @date 2024/08/31
 */
@Slf4j
public class KcToolkit {

    static {
        String userHome = System.getProperty("user.dir");
        String xmapFile = userHome + File.separator + "xmap";
        File file = new File(xmapFile);
        if (!file.exists()) {
            file.mkdirs();
            log.info("[kc] mk xmap dir {}", file.getAbsolutePath());
            new File(xmapFile + File.separator + "科聪xmap文件放这里只能放一个.txt");
        }
    }


    public static Robot robot;
    //本地udp客户端
    public static UdpClient client;
    //科聪变量读写端口
    private static final int varPort = 17800;
    //科聪控制端口
    private static final int controlPort = 17804;

    //科聪控制器ip 连接内部网络时的默认ip
    public static String kcControlIp = "192.168.100.178";
    //科聪控制器ip 连接内部网络时的默认ip
    public static String kcVarIp = "192.168.100.200";




    //获取消息结果
    public static final ConcurrentHashMap<Short, CompletableFuture<Boolean>> requestMap = new ConcurrentHashMap<>();


    public static boolean init(String ip) {
        //初始化ip
        List<String> localIps = getLocalIps();
        boolean isInControllerWifi = localIps.stream().anyMatch(localIp -> localIp.contains("192.168.100."));
        if (!isInControllerWifi && StrUtil.isNotBlank(ip)) {
            // 连接控制器wifi时要使用内部固定ip发送命令
            // 如果是连接交换机或者 ap,由于控制器有映射ip，要使用映射的ip
            kcControlIp = ip;
            kcVarIp = ip;
        }
        if (client == null) {
            log.info("[kc]controller ip:{},plc ip:{}", kcControlIp, kcVarIp);
            //创建针对科聪控制器的 udp 客户端
            client = new UdpClient("kc", new ChannelInitializer<NioDatagramChannel>() {
                @Override
                protected void initChannel(NioDatagramChannel nioDatagramChannel) throws Exception {
                    nioDatagramChannel.pipeline().addLast("decoder", new KcDecoder());
                    nioDatagramChannel.pipeline().addLast(new KcMessageHandler());
                }
            });
            NetworkServiceThreadPool.creatNetworkServerOrClient(() -> {
                try {
//                    //切离线 因为如果直接关机的话，操作不了数据库，所以要开机的时候先改成离线再去改上线
                    RobotOnlineEventDto dto = new RobotOnlineEventDto();
//                    dto.setOnlineStatus(0);
//                    SpringUtil.getApplicationContext().publishEvent(dto);
                    waitSystemUpTime();
                    //切在线
                    dto.setOnlineStatus(1);
                    SpringUtil.getApplicationContext().publishEvent(dto);
                    client.bindAsync();
                } catch (Exception e) {
                    log.error("[kc]udp client bind error", e);
                }
            });
        }
        return true;
    }


    /*******************************************************************************************************************************/


    public static boolean writeVar(String varName, DataTypeEnum typeEnum, String varValue, boolean waitForResponse) {
        if (!checkNet()) {
            return false;
        }
        int totalLength = 272;
        ByteBuffer buffer = ByteBuffer.allocate(totalLength);
        buffer.order(ByteOrder.LITTLE_ENDIAN);
        //变量名
        byte[] nameBytes = varName.getBytes(StandardCharsets.US_ASCII);
        byte[] adjustedBytes = new byte[16];
        System.arraycopy(nameBytes, 0, adjustedBytes, 0, Math.min(nameBytes.length, adjustedBytes.length));
        buffer.put(adjustedBytes);
        if (Objects.equals(typeEnum.getType(), DataTypeEnum.UDINT.getType())) {
            buffer.putInt(Integer.parseInt(varValue));
        }
        if (Objects.equals(typeEnum.getType(), DataTypeEnum.REAL.getType())) {
            buffer.putFloat(Float.parseFloat(varValue));
        }
        if (Objects.equals(typeEnum.getType(), DataTypeEnum.INT.getType())) {
            buffer.putShort(Short.parseShort(varValue));
        }
        if (Objects.equals(typeEnum.getType(), DataTypeEnum.BOOL.getType())) {
            buffer.put(Byte.parseByte(varValue));
        }
        // 提取数组
        byte[] data = new byte[totalLength];
        buffer.rewind();
        buffer.get(data);
        buffer.clear();
        KcMessage kcMessage = new KcMessage((byte) 0x00, data);
        client.sendMessage(kcMessage.message2bytes(), kcVarIp, varPort);
        if (!waitForResponse) {
            return true;
        }
        return waitForResponse(kcMessage);
    }


    public static String readVar(String varName) {
        if (!checkNet()) {
            return "";
        }
        if(!DataTypeEnum.VAR_NAME_ENUM_MAP.containsKey(varName)){
            return "";
        }
        int totalLength = 16;
        ByteBuffer buffer = ByteBuffer.allocate(totalLength);
        buffer.order(ByteOrder.LITTLE_ENDIAN);
        //变量名
        byte[] nameBytes = varName.getBytes(StandardCharsets.US_ASCII);
        byte[] adjustedBytes = new byte[16];
        System.arraycopy(nameBytes, 0, adjustedBytes, 0, Math.min(nameBytes.length, adjustedBytes.length));
        buffer.put(adjustedBytes);
        // 提取数组
        byte[] data = new byte[totalLength];
        buffer.rewind();
        buffer.get(data);
        buffer.clear();
        KcMessage kcMessage = new KcMessage((byte) 0x01, data);
        //log.info("[kc] send read var:{}",bytesToHexString(kcMessage.message2bytes()));
        client.sendMessage(kcMessage.message2bytes(), kcVarIp, varPort);
        boolean result = waitForResponse(kcMessage);
        if (!result) {
            //log.warn("[kc] read var:{} failed", varName);
        }
        return KcCacheUtil.getVarCache(varName);
    }


    /**
     * 切换车辆控制器的运行模式，手动模式下可以手动定位，自动模式下可以导航控制
     *
     * @param mode 0：切换至手动模式 1：切换至自动模式
     * @return true-成功 false-失败
     */
    public static boolean changeRunningMode(Integer mode) {
        if (!checkNet()) {
            return false;
        }
        byte[] data = new byte[]{
                (byte) (mode & 0xff), 0x00, 0x00, 0x00
        };
        KcMessage kcMessage = new KcMessage((byte) 0x11, data);
        client.sendMessage(kcMessage.message2bytes(), kcControlIp, controlPort);
        boolean result = waitForResponse(kcMessage);
        log.info("[kc]切换手自动模式:{},{}", Objects.equals(mode, 0) ? "手动" : "自动", result);
        return result;
    }

    /**
     * [手动模式]下重新设置机器人的位置，设置后机器人运行状态中的定位状态会先变成[定位中]，然后变成[定位完成]
     *
     * @param x   坐标x
     * @param y   坐标y
     * @param yaw 朝向角
     * @return true-成功 false-失败
     */
    public static boolean setInitPos(double x, double y, double yaw) {
        if (!checkNet()) {
            return false;
        }
        byte[] xBytes = ByteUtil.doubleToBytes(x, ByteOrder.LITTLE_ENDIAN);
        byte[] yBytes = ByteUtil.doubleToBytes(y, ByteOrder.LITTLE_ENDIAN);
        byte[] yawBytes = ByteUtil.doubleToBytes(yaw, ByteOrder.LITTLE_ENDIAN);
        byte[] data = new byte[24];
        System.arraycopy(xBytes, 0, data, 0, 8);
        System.arraycopy(yBytes, 0, data, 8, 8);
        System.arraycopy(yawBytes, 0, data, 16, 8);
        KcMessage kcMessage = new KcMessage((byte) 0x14, data);
        client.sendMessage(kcMessage.message2bytes(), kcControlIp, controlPort);
        boolean result = waitForResponse(kcMessage);
        log.info("[kc]下发初始位置:({},{},{}),{}", x, y, yaw, result);
        return result;
    }


    /**
     * 以不指定路径的方式导航到路径点
     *
     * @param navCmd   操作0：开始导航  1：取消导航 2：暂停导航 3：继续导航 4：创建导航任务并暂停导航
     * @param targetId 路径点 ID，字符串格式  [导航方式为 0 时使用]
     * @return true-成功 false-失败
     */
    public static boolean navToPosControl(int navCmd, String targetId) {
        boolean result = navControl(navCmd, 0, 0, 0, targetId, 0, 0, 0f, 0f, 0f, (short) 0, "");
        if (navCmd == 0) {
            log.info("[kc] nav to pos:[{}],{}", targetId, result);
        } else {
            log.info("[kc] nav cmd:[{}],{}", navCmd, result);
        }
        return result;
    }


    /**
     * 控制机器人导航到指定点[最原始的接口]
     *
     * @param navCmd               操作0：开始导航  1：取消导航 2：暂停导航 3：继续导航 4：创建导航任务并暂停导航
     * @param navMethod            导航方式 0：导航到路径点(端点) 1：导航到路径上的点（线上的某一点，厂商不建议用）
     * @param isPathDefined        是否指定导航路径 0-不指定 1-指定
     * @param isTrafficManaged     是否启用交通管理 0-不启用 1-启用（厂商的调度软件）
     * @param targetId             路径点 ID，字符串格式  [导航方式为 0 时使用]
     * @param targetPathStartPosId 目标路径起点 ID   [导航方式为 1 时使用]
     * @param targetPathEndPosId   目标路径终点 ID  [导航方式为 1 时使用]
     * @param targetPosX           目标点坐标 x，单位 m  [导航方式为 1 时使用]
     * @param targetPosY           目标点坐标 y，单位 m  [导航方式为 1 时使用]
     * @param targetPosYaw         目标点坐标 theta，单位 rad  [导航方式为 1 时使用]
     * @param definedPathPosCount  指定路径的路径数量 [指定路径时使用]
     * @param definedPathPosIds    指定路径的所有路径点 ID，包括起点和终点，用空格隔开 [指定路径时使用]
     * @return true-成功 false-失败
     */
    private static boolean navControl(int navCmd,
                                      int navMethod,
                                      int isPathDefined,
                                      int isTrafficManaged,
                                      String targetId,
                                      int targetPathStartPosId,
                                      int targetPathEndPosId,
                                      float targetPosX,
                                      float targetPosY,
                                      float targetPosYaw,
                                      short definedPathPosCount,
                                      String definedPathPosIds) {
        if (!checkNet()) {
            return false;
        }
        targetId = StrUtil.isBlank(targetId) ? "0" : targetId;
        int totalLength = 432;
        ByteBuffer buffer = ByteBuffer.allocate(totalLength);
        buffer.order(ByteOrder.LITTLE_ENDIAN);
        buffer.put((byte) (navCmd & 0xff));
        buffer.put((byte) (navMethod & 0xff));
        buffer.put((byte) (isPathDefined & 0xff));
        buffer.put((byte) (isTrafficManaged & 0xff));
        byte[] targetIdAsciiBytes = targetId.getBytes(StandardCharsets.US_ASCII);
        byte[] adjustedBytes = new byte[8];
        System.arraycopy(targetIdAsciiBytes, 0, adjustedBytes, 0, Math.min(targetIdAsciiBytes.length, adjustedBytes.length));
        buffer.put(adjustedBytes);
        buffer.putShort((short) (targetPathStartPosId));
        buffer.putShort((short) targetPathEndPosId);
        buffer.putFloat(targetPosX);
        buffer.putFloat(targetPosY);
        buffer.putFloat(targetPosYaw);
        buffer.putShort((short) 0);//保留位
        buffer.putShort(definedPathPosCount);
        if (StrUtil.isNotBlank(definedPathPosIds)) {
            String[] ids = definedPathPosIds.trim().split(" ");
            for (int i = 0; i < ids.length; i++) {
                buffer.putShort(Short.parseShort(ids[i]));
            }
        }
        // 提取数组
        byte[] data = new byte[totalLength];
        buffer.rewind();
        buffer.get(data);
        buffer.clear();
        KcMessage kcMessage = new KcMessage((byte) 0x16, data);
        client.sendMessage(kcMessage.message2bytes(), kcControlIp, controlPort);
        return waitForResponse(kcMessage);
    }

    /**
     * 查询机器人运行状态 高频发送udp可能有丢包
     *
     * @return KcRobotRunningStatus 运行状态实体类
     */
    public static KcRobotRunningStatus queryRunningStatus() {
        if (!checkNet()) {
            return KcRobotRunningStatus.getDefault();
        }
        KcMessage kcMessage = new KcMessage((byte) 0x17);
        client.sendMessage(kcMessage.message2bytes(), kcControlIp, controlPort);
        KcRobotRunningStatus kcRobotRunningStatus = (KcRobotRunningStatus) KcCacheUtil.get(KcCacheUtil.KEY_RUNNING_STATUS);
        if (kcRobotRunningStatus == null) {
            kcRobotRunningStatus = KcRobotRunningStatus.getDefault();
        }
        return kcRobotRunningStatus;
    }


    /**
     * 查询导航状态
     *
     * @return 导航状态实体类
     */
    public static KcNavStatus queryNavStatus() {
        if (!checkNet()) {
            return KcNavStatus.getDefault();
        }
        //
        KcMessage kcMessage = new KcMessage((byte) 0x1D);
        client.sendMessage(kcMessage.message2bytes(), kcControlIp, controlPort);
        waitForResponse(kcMessage);
        KcNavStatus kcNavStatus = (KcNavStatus) KcCacheUtil.get(KcCacheUtil.KEY_NAV_STATUS);
        if (kcNavStatus == null) {
            kcNavStatus = KcNavStatus.getDefault();
        }
        //kcMessage = new KcMessage((byte) 0xAF);
        //client.sendMessage(kcMessage.message2bytes(), kcControlIp, controlPort);
        //waitForResponse(kcMessage);
        //kcNavStatus = (KcNavStatus) KcCacheUtil.get(KcCacheUtil.KEY_NAV_STATUS);
        //if (kcNavStatus == null) {
        //    kcNavStatus = KcNavStatus.getDefault();
        //}
        return kcNavStatus;
    }

    public static RobotMap parseMapInfo() {
        try {
            String content = parseXMapFile();
            if (StrUtil.isBlank(content)) {
                return null;
            }
            //地图
            Document document = DocumentHelper.parseText(content);
            //String type = document.getRootElement().element("header").attributeValue("type");
            String uuid = document.getRootElement().element("header").attributeValue("uuid");
            String name = document.getRootElement().element("header").attributeValue("name");
            String resolution = document.getRootElement().element("header").attributeValue("resolution");
            String minPosX = document.getRootElement().element("header").element("min_pos").attributeValue("x");
            String minPosY = document.getRootElement().element("header").element("min_pos").attributeValue("y");
            String maxPosX = document.getRootElement().element("header").element("max_pos").attributeValue("x");
            String maxPosY = document.getRootElement().element("header").element("max_pos").attributeValue("y");
            RobotMap robotMap = new RobotMap();
            robotMap.setMapName(name);
            robotMap.setMapCode(uuid);
            robotMap.setMapOriginx(BigDecimal.valueOf(Double.parseDouble(minPosX)).add(new BigDecimal(maxPosX)).divide(new BigDecimal(2), 3, RoundingMode.HALF_UP).floatValue());
            robotMap.setMapOriginy(BigDecimal.valueOf(Double.parseDouble(minPosY)).add(new BigDecimal(maxPosY)).divide(new BigDecimal(2), 3, RoundingMode.HALF_UP).floatValue());
            robotMap.setMaxX(Float.parseFloat(maxPosX));
            robotMap.setMaxY(Float.parseFloat(maxPosY));
            robotMap.setMinX(Float.parseFloat(minPosX));
            robotMap.setMinY(Float.parseFloat(minPosY));
            robotMap.setMapResolution(Float.parseFloat(resolution));
            robotMap.setType(1);
            robotMap.setFilePath("/file/map/" + robotMap.getMapCode() + ".png");
            return robotMap;
        } catch (DocumentException e) {
            log.info("[kc] parse map xml error", e);
        }
        return null;
    }

    public static List<RobotMapPos> parsePosInfo() {
        String content = parseXMapFile();
        if (StrUtil.isBlank(content)) {
            return null;
        }
        ArrayList<RobotMapPos> list = new ArrayList<>();
        try {
            //地图
            Document document = DocumentHelper.parseText(content);
            String name = document.getRootElement().element("header").attributeValue("name");
            //前置点关系
            Multimap<String, String> multimap = ArrayListMultimap.create();
            List<Element> advancedCurve = document.getRootElement().elements("advanced_curve");
            for (Element element : advancedCurve) {
                String direction = element.attributeValue("Direction");
                String startId = element.element("start_pos").attributeValue("id");
                String endId = element.element("end_pos").attributeValue("id");
                if (Objects.equals(direction, "both")) {
                    //前进
                    multimap.put(endId, startId);
                    multimap.put(startId, endId);
                } else if (Objects.equals(direction, "forward")) {
                    multimap.put(endId, startId);
                } else {
                    multimap.put(startId, endId);
                }
            }
            //点位
            List<Element> advancedPoint = document.getRootElement().elements("advanced_point");
            for (Element element : advancedPoint) {
                String className = element.attributeValue("class_name");
                String instanceName = element.attributeValue("instance_name");
                String id = element.attributeValue("id");
                String angle = element.attributeValue("angle");
                String x = element.element("pos").attributeValue("x");
                String y = element.element("pos").attributeValue("y");
                RobotMapPos robotMapPos = new RobotMapPos();
                robotMapPos.setPosX(Double.parseDouble(x));
                robotMapPos.setPosY(Double.parseDouble(y));
                robotMapPos.setPosZ(0.0D);
                robotMapPos.setYaw(Double.parseDouble(angle));
                robotMapPos.setCode(id);
                robotMapPos.setName(instanceName);
                robotMapPos.setSplitFlag(0);
                robotMapPos.setMapName(Optional.ofNullable(robot).map(Robot::getMapCode).orElse("科聪地图"));
                robotMapPos.setMapCode(Optional.ofNullable(robot).map(Robot::getMapCode).orElse("kc_map"));
                Collection<String> strings = multimap.get(id);
                if (strings != null && !strings.isEmpty()) {
                    robotMapPos.setPreCode(StrUtil.join(",", strings));
                }
                list.add(robotMapPos);
            }
        } catch (DocumentException e) {
            log.info("[kc] parse map xml error", e);
        }
        return list;
    }


    private static String parseXMapFile() {
        String userHome = System.getProperty("user.dir");
        String xmapFile = userHome + File.separator + "xmap";
        File file = new File(xmapFile);
        if (!file.exists()) {
            return null;
        }
        File[] files = file.listFiles(f -> f.getName().contains(".xmap"));
        if (files == null || files.length == 0) {
            return null;
        }
        Optional<File> first = Arrays.stream(files).findFirst();
        return FileUtil.readString(first.get().getAbsolutePath(), StandardCharsets.UTF_8);
    }


    /****************************************************************************************************************************/


    /**
     * 检测通讯是否正常
     *
     * @return true-正常 false-异常
     */
    private static boolean checkNet() {
        if (client == null) {
            log.warn("[kc] udp client is null");
            return false;
        }
        if (StrUtil.isBlank(kcControlIp)) {
            log.warn("[kc] kcControlIp is null");
            return false;
        }
        //log.warn("[kc] remote ip : {} is not reachable", kcControlIp);
        return NetUtil.ping(kcControlIp, 2000);
    }

    /**
     * 将字节数组转为十六进制字符串
     *
     * @param bytes 字节数组
     * @return 十六进制字符串
     */
    public static String bytesToHexString(byte[] bytes) {
        StringBuilder hexString = new StringBuilder();
        for (int i = 0; i < bytes.length; i++) { // 将字节转为无符号的十六进制字符串
            String hex = String.format("%02X", bytes[i] & 0xFF);
            hexString.append(hex);
            if (i < bytes.length - 1) {
                hexString.append(" ");
            }
        }
        return hexString.toString();
    }


    public static boolean waitForResponse(KcMessage kcMessage) {
        CompletableFuture<Boolean> future = new CompletableFuture<>();
        requestMap.put(kcMessage.getSequence(), future);
        try {
            return future.get(500, TimeUnit.MILLISECONDS);
        } catch (InterruptedException | ExecutionException e) {
            throw new RuntimeException(e);
        } catch (TimeoutException e) {
            //log.warn("[kc]seq:{},cmd:{} timeout", kcMessage.getSequence(), kcMessage.getCmdHexStr());
        }
        return false;
    }

    public static void release(KcMessage kcMessage) {
        CompletableFuture<Boolean> future = requestMap.remove(kcMessage.getSequence());
        if (future != null) {
            future.complete(Objects.equals(kcMessage.getExecuteCode(), (byte) 0x00));
            //log.debug("[kc]seq:{},cmd:{},release",kcMessage.getSequence(),kcMessage.getCmdHexStr());
        }
    }


    private static List<String> getLocalIps() {
        List<String> ips = new ArrayList<>();
        try {
            // 获取所有网络接口
            Enumeration<NetworkInterface> networkInterfaces = NetworkInterface.getNetworkInterfaces();
            while (networkInterfaces.hasMoreElements()) {
                NetworkInterface networkInterface = networkInterfaces.nextElement();
                // 获取网络接口上的所有 IP 地址
                Enumeration<InetAddress> inetAddresses = networkInterface.getInetAddresses();
                while (inetAddresses.hasMoreElements()) {
                    InetAddress inetAddress = inetAddresses.nextElement();
                    // 只输出 IPv4 地址
                    if (inetAddress instanceof java.net.Inet4Address) {
                        log.debug("[kc]本机网络接口:{}", networkInterface.getName());
                        log.debug("[kc]本机IPv4地址:{}", inetAddress.getHostAddress());
                        ips.add(inetAddress.getHostAddress());
                    }
                }
            }
        } catch (SocketException e) {
            e.printStackTrace();
        }
        return ips;
    }


    private static void waitSystemUpTime() {
        try {
            String osName = System.getProperty("os.name").toLowerCase();
            log.info("os.name:{}",osName);
            // 判断是否为 Linux 系统
            if (osName.contains("linux")) {
                Process process = Runtime.getRuntime().exec("uptime -s");
                BufferedReader reader = new BufferedReader(new InputStreamReader(process.getInputStream()));
                String line = reader.readLine();
                DateTime upDate = DateUtil.parse(line, "yyyy-MM-dd HH:mm:ss");
                long betweenSecond = DateUtil.between(upDate, new Date(), DateUnit.SECOND);
                log.info("[kc] 开机时间:{},已开机{}秒", line, betweenSecond);
                if (betweenSecond < 60 * 2) {
                    log.info("[kc]开机不足 2 分钟，延迟和控制器的通讯");
                    TimeUnit.SECONDS.sleep(60 * 2 - betweenSecond);
                }
            } else {
                //  Windows
            }
        } catch (Exception e) {
            log.info("[kc]check system up time error:{}", e.getMessage());
        }
    }

}
